import Agent from "./Agent";
import KdTree from "./KdTree";
import Line from "./Line";
import Obstacle from "./Obstacle";
import RVOMath from "./RVOMath";
import Vector2D from "./Vector2D";

export default class Simulator {
	agents: Agent[] = [];
	obstacles: Obstacle[] = [];
	private goals: Vector2D[] = [];
	kdTree: KdTree = new KdTree();

	timeStep = 0.25;

	private defaultAgent: Agent; // Agent

	private time = 0;

	constructor() {
		this.kdTree.simulator = this;
		this.kdTree.MAXLEAF_SIZE = 1000;
	}
	getGlobalTime(): number {
		return this.time;
	}

	getNumAgents(): number {
		return this.agents.length;
	}

	getTimeStep(): number {
		return this.timeStep;
	}

	setAgentPrefVelocity(i: number, prefVelocity: Vector2D) {
		this.agents[i].prefVelocity = prefVelocity;
	}

	setAgentPosition(i: number, position: Vector2D) {
		this.agents[i].position = position;
	}

	setAgentGoal(i: number, goal: Vector2D) {
		this.goals[i] = goal;
	}

	setTimeStep(timeStep: number) {
		this.timeStep = timeStep;
	}

	getAgentPosition(i: number): Vector2D {
		return this.agents[i].position;
	}

	getAgentPrefVelocity(i: number): Vector2D {
		return this.agents[i].prefVelocity;
	}

	getAgentVelocity(i: number): Vector2D {
		return this.agents[i].velocity;
	}

	getAgentRadius(i: number): number {
		return this.agents[i].radius;
	}

	getAgentOrcaLines(i: number): Line[] {
		return this.agents[i].orcaLines;
	}

	addAgent(
		position: Vector2D,
		radius: number = null,
		maxSpeed: number = null,
		velocity: Vector2D = null	
	) {
		if (!this.defaultAgent) {
			throw new Error("no default agent");
		}

		if (!position) position = new Vector2D(0, 0);

		var agent = new Agent();

		agent.position = position;
		agent.maxNeighbors = this.defaultAgent.maxNeighbors;

		agent.radius = radius || this.defaultAgent.radius;
		agent.maxSpeed = maxSpeed || this.defaultAgent.maxSpeed;
		agent.neighborDist = this.defaultAgent.neighborDist;
		agent.timeHorizon = this.defaultAgent.timeHorizon;
		agent.timeHorizonObst = this.defaultAgent.timeHorizonObst;
		agent.velocity = velocity || this.defaultAgent.velocity;
		agent.simulator = this;

		agent.id = this.agents.length;
		this.agents.push(agent);
		this.goals.push(position);

		return this.agents.length - 1;
	}

	/**
	 *
	 * @param neighborDist 在寻找周围邻居的搜索距离，这个值设置过大，会让小球在很远距离时做出避障行为
	 * @param maxNeighbors 寻找周围邻居的最大数目，这个值设置越大，最终计算的速度越精确，但会增大计算量
	 * @param timeHorizon 代表计算动态的物体时的时间窗口
	 * @param timeHorizonObst 代表计算静态的物体时的时间窗口，比如在RTS游戏中，小兵向城墙移动时，没必要做出避障，这个值需要 设置得很小
	 * @param radius 代表计算ORCA时的小球的半径，这个值不一定与小球实际显示的半径一样，偏小有利于小球移动顺畅
	 * @param maxSpeed 小球最大速度值
	 * @param velocity 小球初始速度
	 */
	setAgentDefaults(
		neighborDist: number,
		maxNeighbors: number,
		timeHorizon: number,
		timeHorizonObst: number,
		radius: number,
		maxSpeed: number,
		velocity: Vector2D
	) {
		if (!this.defaultAgent) {
			this.defaultAgent = new Agent();
		}

		this.defaultAgent.maxNeighbors = maxNeighbors;
		this.defaultAgent.maxSpeed = maxSpeed;
		this.defaultAgent.neighborDist = neighborDist;
		this.defaultAgent.radius = radius;
		this.defaultAgent.timeHorizon = timeHorizon;
		this.defaultAgent.timeHorizonObst = timeHorizonObst;
		this.defaultAgent.velocity = velocity;
		this.defaultAgent.simulator = this;
	}

	run(dt: number) {
		this.kdTree.buildAgentTree();

		for (var i = 0; i < this.getNumAgents(); i++) {
			this.agents[i].computeNeighbors();
			this.agents[i].computeNewVelocity();
			this.agents[i].update();
		}
		this.time += dt;
		// this.time += this.timeStep;
	}

	reachedGoal(): boolean {
		let pos: Vector2D;
		for (var i = 0, len = this.getNumAgents(); i < len; ++i) {
			pos = this.getAgentPosition(i);
			if (RVOMath.absSq(this.goals[i].minus(pos)) > RVOMath.RVO_EPSILON) {
				return false;
			}
		}
		return true;
	}

	addGoals(goals: Vector2D[]) {
		this.goals = goals;
	}

	getGoal(goalNo: number): Vector2D {
		return this.goals[goalNo];
	}

	addObstacle(vertices: Vector2D[]): number {
		if (vertices.length < 2) {
			return -1;
		}

		var obstacleNo = this.obstacles.length;

		for (var i = 0, len = vertices.length; i < len; ++i) {
			var obstacle = new Obstacle();
			obstacle.point = vertices[i];
			if (i != 0) {
				obstacle.previous = this.obstacles[this.obstacles.length - 1];
				obstacle.previous.next = obstacle;
			}
			if (i == vertices.length - 1) {
				obstacle.next = this.obstacles[obstacleNo];
				obstacle.next.previous = obstacle;
			}
			obstacle.unitDir = RVOMath.normalize(
				vertices[i == vertices.length - 1 ? 0 : i + 1].minus(vertices[i])
			);

			if (vertices.length == 2) {
				obstacle.isConvex = true;
			} else {
				obstacle.isConvex =
					RVOMath.leftOf(
						vertices[i == 0 ? vertices.length - 1 : i - 1],
						vertices[i],
						vertices[i == vertices.length - 1 ? 0 : i + 1]
					) >= 0;
			}

			obstacle.id = this.obstacles.length;

			this.obstacles.push(obstacle);
		}

		return obstacleNo;
	}

	processObstacles() {
		this.kdTree.buildObstacleTree();
	}

	queryVisibility(point1: Vector2D, point2: Vector2D, radius: number): boolean {
		return this.kdTree.queryVisibility(point1, point2, radius);
	}

	getObstacles(): Obstacle[] {
		return this.obstacles;
	}
}
